#include "Scene.h"
#include "ObjectPopper.h"
#include "trackView.h"


Scene::Scene()
{
	float initBoxCoor[3];
	float outSceneCoordinate[3] = {-100, -100, 0};
	float redColor[3] = {255, 0, 0};

	addObject(new SceneObject(outSceneCoordinate)); // random / calibration objects
	addObject(new SceneObject(outSceneCoordinate, redColor)); // used to display the estimated region

	// ozgur - 07-04-2009 -- 22:01
	initBoxCoor[0] = 1/2.0 * regionWidth;
	initBoxCoor[1] = 1/2.0 * regionHeight;
	initBoxCoor[2] = 0;
	(sceneObjects[0])->translate(initBoxCoor);

#ifndef DEBUG
	initHeadTrack();
#endif
	sceneCalibrated = false;

#ifdef DEBUG
	ifstream infile("in.txt");

	FreeTrackData data;
	while(infile.peek() != EOF)
	{
		infile >> data.x >> data.y >> data.z >> data.yaw >> data.pitch >> data.roll;
		headTrackData.push_back(data);
	}

	
#endif
	dataIndex = 4; 
	calculateRegionCenters();

}

Scene::~Scene()
{
	killHeadTrack();

	//delete[][] regionAngles;
	//delete[][] regionCenters;
	headTrackData.empty();
	headTrackData.clear();
	sceneObjects.empty();
}

void Scene::addObject(SceneObject* obj)
{
	sceneObjects.push_back(obj);
}

void Scene::removeLastObject()
{
	this->sceneObjects.pop_back();
}

void Scene::draw()
{
	if (ObjectPopper::eventTriggered == 1)
	{
		if (sceneCalibrated)
		{
			//cout << "SceneCalibrated! eTrig: " << ObjectPopper::eventTriggered << " eCnt: " << ObjectPopper::eventCount << endl;
			int* regionId = new int[2];
#ifdef DEBUG
			data = headTrackData[dataIndex];
			if (dataIndex < headTrackData.size()-1)
				dataIndex ++;
#endif
			/* Retrieve head track data and save */ 
#ifndef DEBUG
			FreeTrackData data = getHeadTrackData();
			headTrackData.push_back(data);
			//FreeTrackData dataToEst;
			data = headTrackData[headTrackData.size()-1];
#endif
			findNearestRegionId(data.yaw, data.pitch, regionId);
			float estimatedCoordinate[3];
			estimatedCoordinate[0] = regionCenters[regionId[0]][regionId[1]][0];
			estimatedCoordinate[1] = regionCenters[regionId[0]][regionId[1]][1];
			estimatedCoordinate[2] = 0;
			sceneObjects[1]->translate(estimatedCoordinate);
		
			delete regionId;
		}
	}
	else if (ObjectPopper::eventTriggered == 0 && ObjectPopper::eventCount > -1)
	{
#ifndef DEBUG
		if (ObjectPopper::eventCount > 0 && 
			ObjectPopper::eventCount <= NUM_EVENT_TYPES * NUM_CALIB_REGIONS_HORIZONTAL * NUM_CALIB_REGIONS_VERTICAL) {
			//cout << "eTrig: " << ObjectPopper::eventTriggered << " eCnt: " << ObjectPopper::eventCount << endl;
			/* Retrieve head track data and save */ 
			FreeTrackData data = getHeadTrackData();
			headTrackData.push_back(data);
		}
#endif	
		/* Update scene at time step */
		update();
	}

	if ( ObjectPopper::eventCount > -1 ){
		//int objCount = sceneObjects.size();

		//for (int i = 0; i < objCount; i++)
		//{
			sceneObjects[0]->draw();
		//}
	}
}

void Scene::update()
{
	float coordinate[3];

	/* First do calibration */
	if (!ObjectPopper::calibrationOver)
	{
	//	cout << "calOverDiil: " << ObjectPopper::eventCount << endl;
		int regionRow = (ObjectPopper::eventCount/NUM_EVENT_TYPES) % NUM_CALIB_REGIONS_HORIZONTAL;
		int regionColumn = (ObjectPopper::eventCount/NUM_EVENT_TYPES) / NUM_CALIB_REGIONS_HORIZONTAL;

		if (regionRow == 0)
			coordinate[0] = 1/2.0 * regionWidth;//(regionRow + 1/2.0) * regionWidth;
		else 
			coordinate[0] = SCREEN_WIDTH - 1/2.0 * regionWidth;//(regionRow + 1/2.0) * regionWidth;

		if (regionColumn == 0)
			coordinate[1] = 1/2.0 * regionHeight;//(regionColumn + 1/2.0) * regionHeight;
		else 
			coordinate[1] = SCREEN_HEIGHT - 1/2.0 * regionHeight;//(regionColumn + 1/2.0) * regionHeight;

		coordinate[2] = 0.0f;
		(sceneObjects[0])->translate(coordinate);
	}
	else
	{
		if (!sceneCalibrated)
		{
			calibrateWithAngles(4);
			sceneCalibrated = true;
			ObjectPopper::aTimer->Interval = 10;
		}
		if ( ObjectPopper::eventCount % 200 == 0 ){
			// cout << "calb ended. " << endl;
			// randomly add an object to the scene
			coordinate[0] = rand() * SCREEN_WIDTH /RAND_MAX;
			coordinate[1] = rand() * SCREEN_HEIGHT /RAND_MAX;
			coordinate[2] = 0;
			if( coordinate[0] > SCREEN_WIDTH - SQUARE_WIDTH/2.f )
				coordinate[0] =  SCREEN_WIDTH - SQUARE_WIDTH/2.f;
			else if( coordinate[0] < SQUARE_WIDTH/2.f )
				coordinate[0] =  SQUARE_WIDTH/2.f;
			if( coordinate[1] > SCREEN_HEIGHT - SQUARE_WIDTH/2.f )
				coordinate[1] =  SCREEN_HEIGHT - SQUARE_WIDTH/2.f;
			else if( coordinate[1] < SQUARE_WIDTH/2.f )
				coordinate[1] =  SQUARE_WIDTH/2.f;

			sceneObjects[0]->colorRandomly();
			(sceneObjects[0])->translate(coordinate);
		}
	}
}


/*******************************************************************************
Calculates and fills the region centers 
params:
yaw1, pitch1 : angles from lower left corner
yaw2, pitch2 : angles from lower right corner
yaw3, pitch3 : angles from upper left corner
yaw4, pitch4 : angles from upper right corner

yaw is the horizontal angle
pitch is the vertical angle
*******************************************************************************/
void Scene::calibrateWithAngles(int numCalibrationPts)
{

	// extract angles from calibratin points 
	float yaw1 = headTrackData[0].yaw;
	float pitch1 = headTrackData[0].pitch;
	float yaw2 = headTrackData[1].yaw;
	float pitch2 = headTrackData[1].pitch;
	float yaw3 = headTrackData[2].yaw;
	float pitch3 = headTrackData[2].pitch; 
	float yaw4 = headTrackData[3].yaw;
	float pitch4 = headTrackData[3].pitch;

	// First, calculate the vertically framing centers because the inner center  
	// points will be calculated as the interpolation of these
	for (int i = 0; i < NUM_REGIONS_VERTICAL; i++)
	{
		// left frame
		regionAngles[i][0][0] = yaw1 + i * (yaw3 - yaw1) / (NUM_REGIONS_VERTICAL - 1);
		regionAngles[i][0][1] = pitch1 + i * (pitch3 - pitch1) / (NUM_REGIONS_VERTICAL - 1);

		// right frame
		regionAngles[i][NUM_REGIONS_HORIZONTAL-1][0] = yaw2 
			+ i * (yaw4 - yaw2) / (NUM_REGIONS_VERTICAL - 1);
		regionAngles[i][NUM_REGIONS_HORIZONTAL-1][1] = pitch2 
			+ i * (pitch4 - pitch2) / (NUM_REGIONS_VERTICAL - 1);

	}

	float pitchLeft; // this will hold the pitch of the left frame in the corresponding row
	float pitchRight; // this will hold the pitch of the left frame in the corresponding row
	float yawLeft; // this will hold the yaw of the left frame in the corresponding row
	float yawRight; // this will hold the yaw of the left frame in the corresponding row

	// calculate inner region centers row by row
	for (int i = 0; i < NUM_REGIONS_VERTICAL; i++)
	{
		yawLeft = regionAngles[i][0][0];
		yawRight = regionAngles[i][NUM_REGIONS_HORIZONTAL-1][0]; 
		pitchLeft = regionAngles[i][0][1];
		pitchRight = regionAngles[i][NUM_REGIONS_HORIZONTAL-1][1]; 

		for (int j = 1; j < NUM_REGIONS_HORIZONTAL-1; j++)
		{	
			regionAngles[i][j][0] = yawLeft 
				+ j * (yawRight - yawLeft) / (NUM_REGIONS_HORIZONTAL-1);	
			regionAngles[i][j][1] = pitchLeft 
				+ j * (pitchRight - pitchLeft) / (NUM_REGIONS_HORIZONTAL-1);
		}
	}

#ifdef DEBUG
	cout << "Calibration angles: \n";
	for (int i = 0; i < NUM_REGIONS_VERTICAL; i++)
	{
		for (int j = 0; j < NUM_REGIONS_HORIZONTAL; j++)
		{
			cout << "region " << i << "-" << j << ": ";
			cout << "yaw: " << regionAngles[i][j][0] << ", " ;
			cout << "pitch: " << regionAngles[i][j][1] << endl;
		
		}
	}
	cout << "--------------------------" << endl;
#endif
}

void Scene::findNearestRegionId(float yaw, float pitch, int* regionId)
{
	// float* regionId = new float[2];

	float minDistance = MAX_DIST;
	float distance;

	for (int i = 0; i < NUM_REGIONS_VERTICAL; i++)
	{
		for (int j = 0; j < NUM_REGIONS_HORIZONTAL; j++)
		{
			distance = pow(regionAngles[i][j][0] - yaw, 2) + pow(regionAngles[i][j][1] - pitch, 2);
			if (minDistance > distance)
			{
				minDistance = distance;
				regionId[0] = i;
				regionId[1] = j;
			}
		}
	}

	cout << "Angles: " << yaw << " " << pitch << endl;
	cout << "Estimated region " << regionId[0] << "," << regionId[1] << endl;

}

void Scene::calculateRegionCenters()
{
	for (int i = 0; i < NUM_REGIONS_VERTICAL; i++)
	{
		for (int j = 0; j < NUM_REGIONS_HORIZONTAL; j++)
		{
			regionCenters[i][j][0] = (j + 1/2.0) * regionWidth;
			regionCenters[i][j][1] = (i + 1/2.0) * regionHeight;
		}
	}

#ifdef DEBUG
	cout << "Region centers: \n";
	for (int i = 0; i < NUM_REGIONS_VERTICAL; i++)
	{
		for (int j = 0; j < NUM_REGIONS_HORIZONTAL; j++)
		{
			cout << "region " << i << "-" << j << ": ";
			cout << "x: " << regionCenters[i][j][0] << ", " ;
			cout << "y: " << regionCenters[i][j][1] << endl;
		
		}
	}
	cout << "--------------------------" << endl;
	
#endif
}
